惯性导航解算程序C++实现

您所在的位置：网站首页 › 国内的cpu ip厂商 › 惯性导航解算程序C++实现

惯性导航解算程序C++实现

#惯性导航解算程序C++实现| 来源: 网络整理| 查看: 265

惯性导航解算程序C++实现

本文为惯性导航解算程序，为sdust本科课程《惯性导航基础》随课实验及课程设计。在此首先感谢石波老师对小组的支持与帮助，另感谢组员对这一项目作出的贡献！

由于编者水平有限，错误在所难免，望读者指正！！！

程序开源地址：https://github.com/ethanjinhuang/inertial-navigation 欢迎收藏！共同交流进步！

文章目录惯性导航解算程序C++实现1 性质、目的和任务1.1性质、目的和任务1.2组织和分工情况 2 重点及内容2.1 欧拉角、方向余弦阵、四元数、等效旋转矢量转换2.2 大地坐标与地心直角坐标相互转换2.3 IMR格式惯导数据的读取与解析2.4 地理坐标系下的姿态更新2.5 地理坐标系下的速度和位置更新2.6 解析粗对准 3 解算程序设计思路原理3.1编译环境概述：3.2程序组织架构：3.2代码设计：3.2.1姿态转换矩阵3.2.2坐标转换3.2.3 IMR文件的读取与解析3.2.4 姿态、速度、位置更新 4 软件概述及功能介绍4.1 概述4.2软件功能概述 5 程序运行介绍

1 性质、目的和任务 1.1性质、目的和任务 1.2组织和分工情况

小组中共三人，分别为hj、pzl和myj，每人协作、独立进行了惯导解算程序的设计。

小组成员分工hj解算程序的算法部分、可视化界面主体功能的实现pzl界面换肤、图标、按钮换颜色和保存图片与日志功能、软件测试myj说明帮助文档的撰写、程序介绍视频、软件测试 2 重点及内容

主要包含以下几个主要内容，分别为：1. 编程实现欧拉角、方向余弦阵、四元数以及等效旋转矢量之间的相互转换；2. 编程实现大地坐标与地心直角坐标相互转换；3. 编程实现IMR格式惯导数据的读取与解析；4. 编程实现地理坐标系下的姿态更新；5.编程实现地理坐标系下的速度和位置更新；6. 编程实现解析粗对准。

2.1 欧拉角、方向余弦阵、四元数、等效旋转矢量转换

参考严恭敏老师《捷联惯导算法与组合导航原理》

2.2 大地坐标与地心直角坐标相互转换

参考严恭敏老师《捷联惯导算法与组合导航原理》

2.3 IMR格式惯导数据的读取与解析

IMR格式的数据读取即通过给定的IMR格式编写读写程序。其数据结构如下表所示：

IMR Header Struct DefinitionWordSize (bytes)TypeDescriptionszHeader8char[8]“$IMURAW0” – NULL terminated ASCII stringbIsIntelOrMotorola1int8_t0 = Intel (Little Endian), default 1 = Motorola (Big Endian)dVersionNumber8doubleInertial Explorer program version number (e.g. 8.80)bDeltaTheta4int32_t0 = Data to follow will be read as scaled angular rates 1 = (default), data to follow will be read as delta thetas, meaning angular increments (i.e. scale and multiply by dDataRateHz to get degrees/second)bDeltaVelocity4int32_t0 = Data to follow will be read as scaled accelerations 1 = (default), data to follow will be read as delta velocities, meaning velocity increments (i.e. scale and multiply by dDataRateHz to get m/s2)dDataRateHz8doubleThe data rate of the IMU in Hz. e.g. 0.01 second data rate is 100 HzdGyroScaleFactor8doubleIf bDeltaTheta == 0, multiply the gyro measurements by this to get degrees/second If bDeltaTheta == 1, multiply the gyro measurements by this to get degrees, then multiply by dDataRateHz to get degrees/seconddAccelScaleFactor8doubleIf bDeltaVelocity == 0, multiply the accel measurements by this to get m/s2 If bDeltaVelocity == 1, multiply the accel measurements by this to get m/s, then multiply by dDataRateHz to get m/s2iUtcOrGpsTime4int32_tDefines the time tags as GPS or UTC seconds of the week 0 = Unknown, will default to GPS 1 = Time tags are UTC seconds of week 2 = Time tags are GPS seconds of weekiRcvTimeOrCorrTime4int32_tDefines whether the time tags are on the nominal top of the second or are corrected for receiver time bias 0 = Unknown, will default to corrected time 1 = Time tags are top of the second 2 = Time tags are corrected for receiver clock biasdTimeTagBias8doubleIf you have a known bias between your GPS and IMU time tags enter it hereszImuName32char[32]Name of the IMU being usedreserved14uint8_t[4]Reserved for future useszProgramName32char[32]Name of calling programtCreate12time_typeCreation time of filebLeverArmValid1boolTrue if lever arms from IMU to primary GNSS antenna are stored in this headerlXoffset4int32_tX value of the lever arm, in millimeterslYoffset4int32_tY value of the lever arm, in millimeterslZoffset4int32_tZ value of the lever arm, in millimetersReserved[354]354int8_t[354]Reserved for future use

The single header, which is a total of 512 bytes long, is followed by a structure of the following type for each IMU measurement epoch:

IMR Record Struct DefinitionWordSizeTypeDescriptionTime8doubleTime of the current measurementgx4int32_tScaled gyro measurement about the IMU X-axisgy4int32_tScaled gyro measurement about the IMU Y-axisgz4int32_tScaled gyro measurement about the IMU Z-axisax4int32_tScaled accel measurement about the IMU X-axisay4int32_tScaled accel measurement about the IMU Y-axisaz4int32_tScaled accel measurement about the IMU Z-axis 2.4 地理坐标系下的姿态更新

参考严恭敏老师《捷联惯导算法与组合导航原理》

2.5 地理坐标系下的速度和位置更新

参考严恭敏老师《捷联惯导算法与组合导航原理》

2.6 解析粗对准

参考严恭敏老师《捷联惯导算法与组合导航原理》

3 解算程序设计思路原理

参考严恭敏老师《捷联惯导算法与组合导航原理》

3.1编译环境概述：

操作系统: Windows 10专业版 19042.685

主要IDE: Microsoft Visual Studio 2019 + Qt Creator 4.11.1(Community)

程序语言：C++ 11

其他实现IDE: PyCharm Professional 2020.3.1, MATLAB 2020a

3.2程序组织架构：

算法的主要实现结构如下图所示：

在这里插入图片描述

3.2代码设计： 3.2.1姿态转换矩阵

欧拉角类的类架构如下所示：

// 欧拉角 class Euler_angle { public: // 三个参数对应 // phi 航向角 // theta 俯仰角 // gamma 横滚角 Vector3d value = Eigen::Vector3d::Zero(); Euler_angle() { value = Eigen::Vector3d::Zero(); } Euler_angle(Euler_angle const &input) { value = input.value; } // 对象初始化 Euler_angle(double phi, double theta, double gamma); // 数值初始化 Euler_angle(Vector3d input_vector); // Vector3d初始化 //transform function Matrix3d EA2DCM(); // 欧拉角 --> 方向余弦阵 Vector4d EA2QV(); // 欧拉角 --> 四元数 Vector3d EA2ERV(); // 欧拉角 --> 等效旋转矢量 // caculate function Matrix3d EA2ssm(); //Skew-symmetric matrix 欧拉角 --> 反对称阵ssm // Basic function void show(); void initialize() { value = Eigen::Vector3d::Zero(); }; //初始化 void operator value = Eigen::Matrix3d::Zero(); } Direct_cosine_matrix(Direct_cosine_matrix& input) { value = input.value; } // 对象初始化 Direct_cosine_matrix(Matrix3d input_matrix); //transform function Vector3d DCM2EA(); // 方向余弦阵 --> 欧拉角 Vector4d DCM2QV(); // 方向余弦阵 --> 四元数 Vector3d DCM2ERV(); // 方向余弦阵 --> 等效旋转矢量 // Basic function void show(); void initialize() { value = Eigen::Matrix3d::Zero(); }; //初始化 void operator value = Eigen::Vector4d::Zero(); } Quaternion_vector(Quaternion_vector& input) { value = input.value; } // 对象初始化 Quaternion_vector(Vector4d input); //transform function Vector3d QV2EA(); //四元数 --> 欧拉角 Matrix3d QV2DCM(); //四元数 --> 方向余弦阵 Vector3d QV2ERV(); //四元数 --> 等效旋转矢量 // Basic function void show(); void initialize() { value = Eigen::Vector4d::Zero(); }; //初始化 void operator value = Eigen::Vector3d::Zero(); } Equivalent_rotation_vector(Equivalent_rotation_vector& input) { value = input.value; } // 对象初始化 Equivalent_rotation_vector(Vector3d input); //transform function Vector4d ERV2QV(); //等效旋转矢量 --> 四元数 Matrix3d ERV2DCM(); //等效旋转矢量 --> 方向余弦阵 Vector3d ERV2EA(); //等效旋转矢量 --> 欧拉角 // Basic function void show(); void initialize() { value = Eigen::Vector3d::Zero(); }; //初始化 void operator double Time; // 当前观测的时间 int32_t gx; // 关于IMU x轴的缩放陀螺测量量 int32_t gy; // 关于IMU y轴的缩放陀螺测量量 int32_t gz; // 关于IMU z轴的缩放陀螺测量量 int32_t ax; // 关于IMU x轴的缩放加速测量 int32_t ay; // 关于IMU y轴的缩放加速测量 int32_t az; // 关于IMU z轴的缩放加速测量 }; struct adj_IMR_Record { double Time; // 当前观测的时间 double gx; // 关于IMU x轴的缩放陀螺测量量 double gy; // 关于IMU y轴的缩放陀螺测量量 double gz; // 关于IMU z轴的缩放陀螺测量量 double ax; // 关于IMU x轴的缩放加速测量 double ay; // 关于IMU y轴的缩放加速测量 double az; // 关于IMU z轴的缩放加速测量 };

IMR数据读取结构

class imr_data { public: IMR_Header* file_header; vector file_data; void read_data(char* filepath); void initial(); private: void read_imr_header(fstream& imrfile, IMR_Header* imrheader); void read_imr_data(fstream& imrfile, vector& adj_data, IMR_Header imr_header); }; 3.2.4 姿态、速度、位置更新 class update { public: // 存放m-1时刻的方向余弦阵，存放m，m-1时刻的采样数据分别于vector[2],vecotr[1]，存放m-1时刻，m-2时刻的位置数据于vector[1],vector[0]，存放m-1,m-2时刻的速度数据于vector[1],vector[0], 存放采样周期 update(Direct_cosine_matrix a1, vector a2, vector a3, vector a4, vector a5, double a7); void initialize(Direct_cosine_matrix a1, vector a2, vector a3, vector a4, vector a5, double a7); // 初始化函数 input_data inputdata; // 输入数据 attitude_updating_data attitude_data; // 姿态更新结果 vector speed_data; // 速度更新结果，通常存放三次速度更新结果，[0] m-2 时刻的速度更新，[1]，m -1 时刻的速度更新，[2]，m 时刻的速度更新 vector position_data; // 位置更新结果，通常存放三次位置更新结果，[0] m-2 时刻的位置，[1]，m-1 时刻的位置，[2]，m 时刻的速度更新 vector position_result; // 存放结果 void attitude_update(); // 姿态更新 void speed_update(); // 速度更新 void position_update(); // 位置更新 void initial_alignment(); // 初始对准 // 定义通用变量 private: double sinl, cosl, tanl; double Rm; double Rn; double Rmh; double Rnh; }; 4 软件概述及功能介绍 4.1 概述

操作系统：Windows 10

IDE：Microsoft Visual Studio 2015

程序语言：C++ 11

主程序文件构成：GPSsolving.h GPSsolving.cpp question.cpp

矩阵类文件构成：Matrix.h Matrix.cpp

4.2软件功能概述

软件功能如下图所示：在这里插入图片描述